import { OrientedBox } from './module/bvh.module.js';
import * as dat from './module/dat.gui.module.js'
import * as three from './module/three.module.js'

three.Object3D.DEFAULT_MATRIX_AUTO_UPDATE = true;
three.Object3D.DEFAULT_MATRIX_WORLD_AUTO_UPDATE = true;

const renderer = new three.WebGLRenderer({
    antialias: true,
    canvas: document.getElementById("canvas")
});
renderer.setClearColor('black');
renderer.setSize(window.innerWidth, window.innerHeight);

const scene = new three.Scene();
const camera = new three.PerspectiveCamera(
    45, window.innerWidth / window.innerHeight, 1, 1000
);
camera.position.set(25, 25, 25);

window.onresize = function () {
    const w = window.innerWidth;
    const h = window.innerHeight;
    camera.aspect = w / h;
    camera.updateProjectionMatrix();
    renderer.setSize(w, h);
};

const ambientLight = new three.AmbientLight('white', 1.0);
const pointLight = new three.PointLight('white', 10.0, 0, 0.0);
pointLight.position.set(20, 20, 20);
pointLight.castShadow = true;
scene.add(ambientLight);
scene.add(pointLight);

const helper = new three.AxesHelper(5);
scene.add(helper);

const materialSettings = {
    metalness: 0.7,
    roughness: 0.3
};

const base = new three.Mesh(
    new three.CylinderGeometry(1.5, 1.2, 0.5, 32),
    new three.MeshStandardMaterial({
        color: 'cyan', ...materialSettings
    })
);
base.position.y = 0.25;
scene.add(base);
pointLight.target = base;

const column = new three.Mesh(
    new three.CylinderGeometry(0.4, 0.4, 5, 16),
    new three.MeshStandardMaterial({
        color: 'gray', ...materialSettings
    })
);
column.position.y = 1.75;
base.add(column);


const armLength = 6;
const joint1 = new three.Mesh(
    new three.SphereGeometry(0.7, 32, 32),
    new three.MeshStandardMaterial({
        color: 'brown', ...materialSettings
    })
);
joint1.position.y = 3.5;
column.add(joint1);

const arm1 = new three.Mesh(
    new three.BoxGeometry(0.5, 0.5, armLength),
    new three.MeshStandardMaterial({
        color: 'yellow', ...materialSettings
    })
);
arm1.position.z = armLength / 2;
joint1.add(arm1);

const joint2 = new three.Mesh(
    new three.SphereGeometry(0.7, 32, 32),
    new three.MeshStandardMaterial({
        color: 'brown', ...materialSettings
    })
);
joint2.position.z = armLength / 2;
arm1.add(joint2);

const arm2 = new three.Mesh(
    new three.BoxGeometry(0.5, 0.5, armLength),
    new three.MeshStandardMaterial({
        color: 'yellow', ...materialSettings
    })
);
arm2.position.z = armLength / 2;
joint2.add(arm2);

const gripperBox = new three.Mesh(
    new three.SphereGeometry(0.7, 32, 32),
    new three.MeshStandardMaterial({
        color: 'silver', ...materialSettings
    })
);
gripperBox.position.z = armLength / 2;
arm2.add(gripperBox);

const gripper1 = new three.Mesh();
{
    const tmpMesh = new three.Mesh(
        new three.ConeGeometry(0.2, 3.0),
        new three.MeshStandardMaterial({
            color: 'orange', ...materialSettings
        })
    );
    tmpMesh.position.y = 1.8;
    gripper1.add(tmpMesh);
}
const gripper2 = gripper1.clone();
const gripper3 = gripper1.clone();
const gripper4 = gripper1.clone();

gripper2.rotation.z = Math.PI / 2;
gripper3.rotation.z = Math.PI;
gripper4.rotation.z = -Math.PI / 2;

gripperBox.add(gripper1);
gripperBox.add(gripper2);
gripperBox.add(gripper3);
gripperBox.add(gripper4);


const gui = new dat.GUI();
const control = {
    baseAngle: 0,
    // In degrees
    joint1Angle: 0,
    joint2Angle: 0,
    gripperAngle: 45
};

gui.add(control, 'baseAngle', -180, 180, 0.1).name('基座旋转');
gui.add(control, 'joint1Angle', -90, 90, 0.1).name('关节1角度');
gui.add(control, 'joint2Angle', -90, 90, 0.1).name('关节2角度');
gui.add(control, 'gripperAngle', 10, 80, 0.1).name('夹爪开合');

document.onwheel = function (event) {
    let fov = camera.fov;
    fov += event.deltaY * 0.01;
    if (fov < 15.0) {
        fov = 15.0;
    }
    if (fov > 75.0) {
        fov = 75.0;
    }
    camera.fov = fov;
};


const testMesh = new three.Mesh(
    new three.BoxGeometry(1, 1, 1),
    new three.MeshStandardMaterial({
        color: 'white', ...materialSettings
    })
);
testMesh.position.set(0, 0, 8);
scene.add(testMesh);

const meshBox = new three.Box3().setFromObject(testMesh);
const meshes = [gripper1.children[0], gripper2.children[0], gripper3.children[0], gripper4.children[0]];
const boxes = [];
for (const mesh of meshes) {
    const box = new OrientedBox(new three.Vector3(), new three.Vector3(), new three.Matrix4());
    boxes.push(box);
}

function collide(box, meshBoxes) {
    for (const meshbox of meshBoxes) {
        if (meshbox.intersectsBox(box)) {
            return true;
        }
    }
    return false;
}

// three.BufferGeometry.prototype.computeBoundsTree = bvh.computeBoundsTree;
// three.BufferGeometry.prototype.disposeBoundsTree = bvh.disposeBoundsTree;
// three.Mesh.prototype.raycast = bvh.acceleratedRaycast;

// function collide(meshA, meshes) {
//     meshA.updateMatrixWorld();
//     const box = new three.Box3().setFromObject(meshA);
//     for (const mesh of meshes) {
//         mesh.updateMatrixWorld();
//         const matrix = new three.Matrix4();
//         matrix.copy(meshA.matrixWorld).invert();
//         matrix.multiply(mesh.matrixWorld);
//         if (mesh.geometry.boundsTree.intersectsBox(box, matrix)) {
//             return true;
//         }
//     }
//     return false;
// }

// function printMatrix(meshes) {
//     for(const mesh of meshes) {
//         mesh.updateMatrixWorld();
//         const zero = new three.Vector4(0,0,0,1);

//     }
// }

// testMesh.geometry.computeBoundsTree();
// const meshes = [gripper1.children[0], gripper2.children[0], gripper3.children[0], gripper4.children[0]];
// for (const mesh of meshes) {
//     mesh.geometry.computeBoundsTree();
// }

function animate() {
    camera.lookAt(0, 0, 0);
    camera.updateProjectionMatrix();

    function rad(deg) {
        return deg * Math.PI / 180.0;
    };

    base.rotation.y = rad(control.baseAngle);
    joint1.rotation.x = rad(control.joint1Angle);
    joint2.rotation.x = rad(control.joint2Angle);

    const gripAngle = rad(90 - control.gripperAngle);
    gripper1.rotation.x = gripAngle;
    gripper2.rotation.y = gripAngle;
    gripper3.rotation.x = -gripAngle;
    gripper4.rotation.y = -gripAngle;

    gripper1.updateWorldMatrix();
    gripper2.updateWorldMatrix();
    gripper3.updateWorldMatrix();
    gripper4.updateWorldMatrix();

    for (var i = 0; i < meshes.length; ++i) {
        boxes[i].matrix = meshes[i].matrixWorld;
    }

    if (collide(meshBox, boxes)) {
        testMesh.material.color.set('white');
    }
    else {
        testMesh.material.color.set('purple');
    }

    renderer.render(scene, camera);
    requestAnimationFrame(animate);
}
animate();